import matplotlib.pyplot as plt


def plot_arrow_a2b(ax, a, b, col='b'):
        dx = b[0]-a[0]
        dy = b[1]-a[1]
        ax.arrow(a[0], a[1], dx, dy, head_width=0.2, head_length=0.5, length_includes_head=True, color=col)
        ax.plot(a[0], a[1])
        ax.plot(b[0], b[1])


def plot_polygon(ax, polygon, text=''):
    """
    根据点绘制多边形
    :param ax:
    :param polygon:
        二维列表，每个存放多边形点，多个多边形放在不同列表中
    :param text:
        图形中间显示的文本
    :return: None
    """
    ax.set_aspect(1)
    plt.grid(color='b', linewidth='0.3', linestyle='--')
    color = ['b', 'g', 'r', 'c', 'm', 'y', 'k', 'w']

    for r in polygon:
        point_num = len(r)
        for i in range(point_num):
            plot_arrow_a2b(ax, r[i], r[(i+1) % point_num], color[i % len(color)])
            plt.text((r[0][0]+r[2][0])/2, (r[0][1]+r[2][1])/2, text, fontsize=15)


def calc_rotate_from_point_matrix(p0, vector0, vector1):
    """
    p0     : 绕点p0旋转
    vector0: 待旋转向量
    vector1: 目标向量
    """
    assert (0 != vector0[0] or 0 != vector0[1]), "输入了零向量"
    assert (0 != vector1[0] or 0 != vector1[1]), "输入了零向量"

    # 计算向量夹角
    cos_angle = (vector0[0] * vector1[0] + vector0[1] * vector1[1]) / \
                (((vector0[0]**2+vector0[1]**2)**0.5) * ((vector1[0]**2+vector1[1]**2)**0.5))
    sin_angle = (1 - cos_angle**2) ** 0.5
    # 向量叉乘
    if (vector1[1] * vector0[0] - vector1[0] * vector0[1]) < 0:
        sin_angle *= -1

    rotate_matrix = [[cos_angle, -sin_angle, (1-cos_angle)*p0[0] + sin_angle*p0[1]],
                     [sin_angle, cos_angle,  (1-cos_angle)*p0[1] - sin_angle*p0[0]],
                     [0, 0, 1]
                     ]
    # print(sin_angle, cos_angle)

    print(f"rotate_shift_matrix: {rotate_matrix}")
    return rotate_matrix


def mul_matrix(a, b):
    a = a[0]
    ra = a[0] * b[0][0] + a[1] * b[0][1] + a[2] * b[0][2]
    rb = a[0] * b[1][0] + a[1] * b[1][1] + a[2] * b[1][2]
    rc = a[0] * b[2][0] + a[1] * b[2][1] + a[2] * b[2][2]
    return ra, rb, rc


def rectangle_shift(p0, p1):
    return p1[0] - p0[0], p1[1] - p0[1]


def main():

    point = [[5, 5],
             [9, 2],
             [10.5, 4],
             [6.5, 7],
             ]

    r1 = point

    # p0->p1向量
    vector_point = (point[1][0]-point[0][0], point[1][1]-point[0][1])

    # 绕点旋转------------------------------
    # 要旋转到的向量,原点(0，0)->(0，-1)
    vector_const = [-1, 0]

    # 绕该点旋转
    # rotate_point = [0, 0]
    rotate_point = point[0]

    # 计算绕点旋转矩阵
    rotate_matrix = calc_rotate_from_point_matrix(rotate_point, vector_point, vector_const)

    # 通过旋转矩阵求旋转后坐标
    r2 = []  # 旋转后矩形
    r3 = []  # 目标在原点的矩形
    for i in range(len(r1)):
        point[i].append(1)
        r2.append(mul_matrix([point[i]], rotate_matrix))
        r3.append(rectangle_shift(r2[0], r2[i]))

    ax = plt.axes()
    plot_polygon(ax, [r1], "r1")
    plot_polygon(ax, [r2], "r2")
    plot_polygon(ax, [r3], "r3")
    plt.show()


if __name__ == '__main__':
    main()
